#include "common.h"

bool m_isThreadWorkBusy = false;
bool currentIsAorB = true;  //current timr,capture thread should capture image A(imgData) or image B(imgDataB)
bool openOdometryByVelocity = false;//true means open
bool openOdometryByDisplacement = true;//true means open
bool openLocaterByDMbar = false;//true means open

uchar imgData[imgSize];
uchar imgDataB[imgSize];

uchar imgRgb[imgRgbSize];

uchar imagePtr[sampledImgSize];
uchar imgMirriorSeam[sampledImgSize];
uchar imgMirriorSeam_copy[sampledImgSize];
uint imgMirriorSeam_copy_copy[sampledImgSize];

uint preComput[imgHeight];
uint preComput_sampled[sampledHeight];
uint preSampleLocat[sampledImgSize];

signed char coor[8][2] = {{0,-1},{-1,-1},{-1,0},{-1,1},{0,1},{1,1},{1,0},{1,-1}};  //1:row 2:col
//signed char coor5X5[16][2] = {{0,2},{-1,2},{-2,2},{-2,1},{-2,0},{-2,-1},{-2,-2},{-1,-2},{0,-2},{1,-2},{2,-2},{2,-1},{2,0},{2,1},{2,2},{1,2}};
signed char coor5X5[16][2] = {{0,-2},{-1,-2},{-2,-2},{-2,-1},{-2,0},{-2,1},{-2,2},{-1,2},{0,2},{1,2},{2,2},{2,1},{2,0},{2,-1},{2,-2},{1,-2}};
line_template line_seg[segNumLimit];

line_template line_seg_mat[60][segNumLimit/2]; //先暂定这么大，可考虑链表
int line_seg_cnt[60];

line_template line_cell[1];
line_template line_sub_cell[1];
int seg_cnt;

float L_lth=97.08;
uchar grayThres =128;//148
pose agvPoseFromDM;
pose agvPoseFromOdometry;

int samImgCnt=0;
int decodeCnt=0;
struct timeval tvNow,tvNowNow;

position samPS[100];
/*定位边准确性检验点*/
const position locP[19]={{19,1},{19,3},{19,5},{19,7},{19,9},{19,11},{19,13},{19,15},{19,17},{19,19},{17,19},{15,19},{13,19},{11,19},{9,19},{7,19},{5,19},{3,19},{1,19}};
/*数据区数据点*/
const position DatP[64]={{13,15},{13,17},{11,15},{11,17},{11,3},{9,15},{9,17},{9,3},
                         {17,3} ,{17,5} ,{15,3} ,{15,5} ,{15,7},{13,3},{13,5},{13,7},
                         {5,7},{5,9},{3,7},{3,9},{3,11},{17,7},{17,9},{17,11},
                         {13,17},{15,17},{13,3},{15,3},{17,3},{13,5},{15,5},{17,5},
                         {9,5},{11,5},{9,7},{11,7},{13,7},{9,9},{11,9},{13,9},
                         {5,9},{7,9},{5,11},{7,11},{9,11},{5,13},{7,13},{9,13},
                         {17,13},{3,13},{17,15},{3,15},{5,15},{17,17},{3,17},{5,17},
                         {11,11},{13,11},{11,13},{13,13},{15,13},{11,15},{13,15},{15,15}
                        };//4-7 位置错误


void initVar()
{
    agvPoseFromDM.x = -1.0;
    agvPoseFromDM.y = -1.0;
    agvPoseFromDM.theta = 0.0;
    m_isThreadWorkBusy = false;
    currentIsAorB = true;
    openOdometryByVelocity = false;
    openOdometryByDisplacement = true;
}

void initLineSeg()
{
    position pos={0,0};
    for(int i=0; i<60; i++)
    {
        line_seg_cnt[i] = 0;
        for(int j=0; j<segNumLimit/2; j++){
            line_seg_mat[i][j].s = pos;
            line_seg_mat[i][j].e = pos;
            line_seg_mat[i][j].pixNum = 0;
        }
    }
}
void calcPreComput()
{
    for(ushort i=0; i<imgHeight; i++)
        preComput[i] = i*imgWidth;
}

void calcPreComput_sampled()
{
    for(ushort i=0; i<sampledHeight; i++)
        preComput_sampled[i] = i*sampledWidth;
}

void calcPreSampleLocat()
{
    for(ushort x=0; x < sampledWidth; x++)
    for(ushort y=0; y < sampledHeight; y++)
    {
        preSampleLocat[y*sampledWidth +x]=y*sampleInterval*imgWidth +x*sampleInterval;
    }
}
/*@brief 叉乘
  @param A 浮点齐次坐标Ａ
  @param B 浮点齐次坐标Ｂ
  @return C 浮点齐次坐标C
*/
Hposition_f cross_fhp_fhp(Hposition_f A, Hposition_f B)
{
    Hposition_f C;
    C.x = A.y*B.scale - A.scale*B.y;
    C.y = A.scale*B.x - A.x*B.scale;
    C.scale = A.x*B.y - A.y*B.x;
    return C;
}
/*@brief 叉乘
  @param A 浮点齐次坐标Ａ
  @param B 浮点齐次坐标Ｂ
  @return C 浮点非齐次坐标C
*/
position_f cross_fhp_fp(Hposition_f A, Hposition_f B)
{
    double temp;
    position_f C;
    temp= A.x*B.y - A.y*B.x;
    C.x = (A.y*B.scale - A.scale*B.y)/temp;
    C.y = (A.scale*B.x - A.x*B.scale)/temp;
    return C;
}
/*@brief 叉乘
  @param A 浮点齐次坐标Ａ
  @param B 浮点齐次坐标Ｂ
  @return C 整型非齐次坐标C
*/
position cross_fhp_ip(Hposition_f A, Hposition_f B)
{
    double temp;
    position C;
    temp= A.x*B.y - A.y*B.x;
    C.col = (int)round((A.y*B.scale - A.scale*B.y)/temp);
    C.row = (int)round((A.scale*B.x - A.x*B.scale)/temp);
    return C;
}

position cross_hp_ip(Hposition A, Hposition B)
{
    float temp;
    position C;
    temp= A.x*B.y - A.y*B.x;
    C.col = (int)round((A.y*B.scale - A.scale*B.y)/temp);
    C.row = (int)round((A.scale*B.x - A.x*B.scale)/temp);
    return C;
}

position_f cross_hp_fp(Hposition A, Hposition B)
{
    float temp;
    position_f C;
    temp= A.x*B.y - A.y*B.x;
    C.x = float((A.y*B.scale - A.scale*B.y))/temp;
    C.y = float((A.scale*B.x - A.x*B.scale))/temp;
    return C;
}

Hposition cross_ip_hp(position A, position B) //默认了尺度为1
{
    Hposition C;
    C.x = A.row - B.row;
    C.y = B.col - A.col;
    C.scale = A.col*B.row - A.row*B.col;
    return C;
}
/*@brief 叉乘
  @param A 非齐次坐标Ａ
  @param B 非齐次坐标Ｂ
  @return C 齐次坐标C
*/
Hposition_f cross_ip_fhp(position A, position B) //默认了尺度为1
{
    Hposition_f C;
    C.x = double(A.row - B.row);
    C.y = double(B.col - A.col);
    C.scale = double(A.col*B.row - A.row*B.col);
    return C;
}

int manhattan(position A, position B)
{
    return (abs(A.col-B.col)+abs(A.row-B.row));
}
/*@brief保存一帧bmp格式图像
  @param szFile 图像文件名
  @param pBmp 图像原始数据
  @param width 图像宽度
  @param height 图像高度
  @param bitCount 像素点大小
*/
int save_to_bmp(char *szFile, uchar* pBmp, int width, int height, int bitCount)
{
    //char header[14]={0x42, 0x4d, 0x36, 0x82, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x36, 0x00, 0x00, 0x00};
    char header[14]={0x42, 0x4d, 0x36, 0xB4, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x36, 0x04, 0x00, 0x00};
    //char header[14]={0x42, 0x4d, 0x36, 0x10, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x36, 0x00, 0x00, 0x00};
    FILE *file=NULL;
    long bmp_size=0;
    ASSERT(szFile!=NULL);
    ASSERT(pBmp!=NULL);
    long *size=(long *)&header[2];

    bmp_size = width*height*(bitCount/8);
    if((file=fopen(szFile, "wb"))==NULL)
    {
        perror("open file error\n");
        return 0;
    }

    // write bmp file header
    BITMAPFILEHEADER bmpHeader;
    BITMAPINFOHEADER bmiHeader;
    RGBQUAD rgbquad[256];
    unsigned long dwFileSize = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER) + sizeof(RGBQUAD)*256 + width*height;

    // bmpHeader.bfSize = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER) + bmp_size;    // BMP file size
    bmpHeader.bfSize = dwFileSize;	    // BMP file size
    bmpHeader.bfType = 0x4D42;			// BMP file type
    // bmpHeader.bfOffBits = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER);            // offset of image data
    bmpHeader.bfOffBits = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER) + sizeof(RGBQUAD)*256;			// offset of image data
    printf("%ld\n",bmpHeader.bfOffBits );
    fflush(stdout);
    bmpHeader.bfReserved1 = 0;			// reservered1
    bmpHeader.bfReserved2 = 0;			// reservered2

    *size = bmpHeader.bfSize;


    bmiHeader.biSize = sizeof(bmiHeader);		// sizeof of this struct
    bmiHeader.biWidth = width;					// image width
    bmiHeader.biHeight = height;				// image height
    bmiHeader.biPlanes = 1;						// set it to 1
    bmiHeader.biBitCount = bitCount;			// bit count
    bmiHeader.biCompression = 0;				// compression type [0= no compression, 1=BI_RLE8, 2=BI_RLE4
    bmiHeader.biSizeImage = 0;			// image data size
    bmiHeader.biXPelsPerMeter = 0;				// X per meter
    bmiHeader.biYPelsPerMeter = 0;				// Y per meter
    bmiHeader.biClrUsed = 256;					// index use or no
    bmiHeader.biClrImportant = 0;				// 0



    for(int i=0;i<256;i++)  //构造灰度图的调色版
    {
        rgbquad[i].rgbBlue = (uchar)i;
        rgbquad[i].rgbGreen = (uchar)i;
        rgbquad[i].rgbRed = (uchar)i;
        rgbquad[i].rgbReserved=0;
    }

    uchar *buff;
    buff = (uchar*)malloc(bmp_size);
    for(int i=height-1; i>=0; i--)
    {
        for(int j=0; j<width; j++)
        {
            buff[i*width+j] = pBmp[(height-1-i)*width+j];
        }
    }

    DEBUGMSG(("bmpHeader=%d, bmiHeader=%d\n", sizeof(BITMAPFILEHEADER), sizeof(BITMAPINFOHEADER)));
    fwrite(header, 1, 14, file);
    // len = 14
    //fwrite(&bmpHeader, 1, sizeof(bmpHeader), file);
    // len = 40
    fwrite(&bmiHeader, 1, sizeof(bmiHeader), file);
    fwrite(&rgbquad, 1, sizeof(RGBQUAD)*256, file);
    fwrite(buff, 1, bmp_size, file);

    free(buff);
    fclose(file);
    return true;
}

int read_from_bmp(char *szFile, uchar* pBmp, int width, int height)
{
    FILE *file=NULL;
    if((file=fopen(szFile, "rb"))==NULL)
    {
        perror("open file error\n");
        return 0;
    }
    char header[14];
    //BITMAPFILEHEADER bmpHeader;
    BITMAPINFOHEADER bmiHeader;
    RGBQUAD rgbquad[256];
    fread(&header, 1, 14, file);
    fread(&bmiHeader, 1, sizeof(bmiHeader), file);
    fread(&rgbquad, 1, sizeof(RGBQUAD)*256, file);
    for(int i=height-1; i>=0; i--)
    {
        fread(pBmp+i*width, 1, width, file);
    }
    return true;
}

void bayer2rgb24_ccjt(unsigned char *dst, unsigned char *src, long width, long height)
{
    unsigned char	*bayer,
                    *image;
    unsigned char	red 	= 0,
                    green	= 0,
                    blue 	= 0;
                    bayer = src;					//raw
                    image = dst;					//rgb24

    unsigned char	hg 	= 0 ,
                    vg 	= 0 ,
                    db1	= 0 ,
                    db2 = 0 ;

    int y = 0;
    int x = 0;
    int yy =0;
    int xx =0;
    char h_flg = false;
    char v_flg = false;
    for(yy = 0; yy < height; yy++ )
    {
        if(h_flg == false)//TRUE)	// the software to get the opposite
        {
            y = height - yy - 1;
        }
        else
        {
            y = yy;
        }
        for(xx = 0; xx < width; xx++ )
        {
            if(v_flg == true)
            {
                x = width - xx -1;
            }
            else
            {
                x = xx;
            }

            if( x == 0 )
            {
                if( y == 0 )				// (0,0) point
                {
                    red 	=  bayer[x + (y+1)*width];
                    green 	= (bayer[x + (y+1)*width] + bayer[x+1 + y*width])/2;
                    blue 	=  bayer[x + y*width];
                }
                else if( y == height - 1 )		// (width,0) point
                {
                    red 	= bayer[x+1 + y*width];
                    green 	= bayer[x 	+ y*width];
                    blue 	= bayer[x 	+ (y-1)*width];
                }
                else if( (y % 2) == 1 )		// row 1 even
                {
                    red 	=  bayer[x+1+ y*width];
                    green 	=  bayer[x 	+ y*width];
                    blue 	= (bayer[x 	+ (y-1)*width] + bayer[x + (y+1)*width])/2;
                }
                else if( (y % 2) == 0 )		// row 1 odd
                {
                    red 	= (bayer[x+1+ (y-1)*width] + bayer[x+1 	+ (y+1)*width])/2;
                    green 	= (bayer[x 	+ (y-1)*width] + bayer[x 	+ (y+1)*width]
                    + bayer[x+1+ y*width]+ bayer[x+1 + y*width])/4;
                    blue 	=  bayer[x 	+  y *width];
                }
            }
///////////////////////////////////////////////////////////////////////////////////////////////////////
            else if( x == width )
            {
                if( y == 0 )				// (0,0) point
                {
                    red 	=  bayer[x 	+ (y+1)*width];
                    green 	= (bayer[x	+ (y)*width]) ;
                    blue 	=  bayer[x-1 + y*width];
                }
                else if( y == height - 1)		// (height,0) point
                {
                    red 	= bayer[x	+ y*width];
                    green 	=(bayer[x 	+ (y-1)*width] + bayer[x-1 	+ y*width])/2;
                    blue 	= bayer[x-1 + (y-1)*width];
                }
                else if( (y % 2) == 1 )		// last row even
                {
                    red 	=  bayer[x 	+ 	y*width];
                    green 	= (bayer[x 	+ 	(y-1)*width] + bayer[x 	+ 	(y+1)*width]
                    + bayer[x-1 	+ y*width] + bayer[x-1 	+ 	y*width])/2;
                    blue 	= (bayer[x-1 	+ (y-1)*width] + bayer[x-1	+(y+1)*width])/2;
                }
                else if( (y % 2) == 0 )		// last row odd
                {
                    red 	= (bayer[x 	+ (y-1)*width] + bayer[x 	+ (y+1)*width])/2;
                    green 	=  bayer[x 	+  y*width] ;
                    blue 	=  bayer[x-1 	+  y *width];
                }
            }
////////////////////////////////////////////////////////////////////////////////////////////
            else if( x % 2 == 1)				// even row
            {
                if( y == 0 )				// left 1 even column
                {
                    red		=  bayer[x + (y+1)*width];
                    green 	=  bayer[x + y*width];
                    blue 	= (bayer[x-1 + y*width] + bayer[x+1 +y*width])/2;
                }
                else if( y == height - 1)		// (height,0) point
                {
                    red 	=  bayer[x  + y*width];
                    green 	= (bayer[x 	+ (y-1)*width] + bayer[x + (y-1)*width]
                            +  bayer[x-1 + y*width] + bayer[x+1	+ y*width])/4;
                    blue 	= (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y-1)*width])/2;
                }
                else if( (y % 2) == 1 )		// even column
                {
                    hg 	= abs(bayer[x   + (y-1)*width] 	- bayer[x   + (y+1)*width]);
                    vg 	= abs(bayer[x-1 + y*width] 		- bayer[x+1 + y*width]);
                    db1 = abs(bayer[x-1 + (y-1)*width] 	- bayer[x+1 + (y+1)*width]);
                    db2 = abs(bayer[x-1 + (y+1)*width] 	- bayer[x+1 + (y-1)*width]);
                    red = bayer[x + y*width];
                    if(hg < vg)
                        green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width])/2;
                    else if(hg > vg)
                        green = (bayer[x-1 + y*width] + bayer[x+1 + y*width])/2;
                    else
                        green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width]
                                +bayer[x-1 +  y*width]	+ bayer[x+1 + 	y*width]) /4 ;
                    if(db1 < db2)
                        blue = (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y+1)*width])/2;
                    else if (db1 > db2)
                        blue = (bayer[x-1 + (y+1)*width] + bayer[x+1 + (y-1)*width]) /2;
                    else
                        blue = (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y+1)*width]
                                + bayer[x+1 + (y-1)*width] + bayer[x-1 + (y+1)*width])/4;
                }
                else if( (y % 2) == 0 )		// odd column
                {
                    red 	= (bayer[x	 + (y-1)*width] + bayer[x + (y+1)*width])/2;
                    green 	=  bayer[x 	 +  y*width];
                    blue 	= (bayer[x-1 +  y *width] + bayer[x+1 +  y*width])/2;
                }
            }
////////////////////////////////////////////////////////////////////////////////////////////
            else if ((x % 2) == 0)			// odd row
            {
                if( y == 0 )				// left 1 even column
                {
                    red 	= (bayer[x-1+ (y+1)*width] + bayer[x+1 + (y+1)*width])/2;
                    green 	= (bayer[x 	+ (y+1)*width] + bayer[x +	(y+1)*width]
                    + bayer[x-1 +	(y)*width] + bayer[x+1 + y*width])/4;
                    blue 	= bayer[x + y*width];
                }
                else if( y == height -1 )		// (width,0) point
                {
                    red 	= (bayer[x-1 + y*width] + bayer[x+1 + y*width])/2;
                    green 	=  bayer[x   + y*width];
                    blue 	=  bayer[x 	 + (y-1)*width];
                }
                else if( (y % 2) == 0 )		// odd column
                {
                    hg 	= abs(bayer[x   + (y-1)*width] 	- bayer[x   + (y+1)*width]);
                    vg 	= abs(bayer[x-1 + y*width] 		- bayer[x+1 + y*width]);
                    db1 = abs(bayer[x-1 + (y-1)*width] 	- bayer[x+1 + (y+1)*width]);
                    db2 = abs(bayer[x-1 + (y+1)*width] 	- bayer[x+1 + (y-1)*width]);
                    blue = bayer[x  + y*width];
                    if(hg < vg)
                        green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width])/2;
                    else if(hg > vg)
                        green = (bayer[x-1 + y*width] + bayer[x+1 + y*width])/2;
                    else
                        green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width]
                               + bayer[x-1 +  y*width]+ bayer[x+1 + y*width] ) /4;
                    if(db1 < db2)
                        red = (bayer[x-1 + 	(y-1)*width] + bayer[x+1 + (y+1)*width])/2;
                    else if (db1 > db2)
                        red = (bayer[x-1 + 	(y+1)*width] + bayer[x+1 + (y-1)*width]) /2;
                    else
                        red = (bayer[x-1 + 	(y-1)*width] + bayer[x+1 + (y+1)*width]
                             + bayer[x+1 + (y-1)*width] + bayer[x-1 + 	(y+1)*width])/4;
                }
                else if( (y % 2) ==1 )		// even column
                {
                    red 	= (bayer[x-1 + y*width] + bayer[x+1 	+ (y)*width])/2;
                    green 	=  bayer[x 	 + y*width];
                    blue 	= (bayer[x 	 +  (y-1) *width] + bayer[x + 	(y+1)*width])/2;
                }
            }
            ///////////////////////////////////////////////////////////////////////////////////////////////////////
            image[0]	= min(max(blue,0),255);
            image[1]	= min(max(green,0),255);
            image[2]	= min(max(red,0),255);
            image += 3;
        }
    }
}

